#include <ros/ros.h>
#include <std_msgs/Float32.h>
#include <serial/serial.h>

serial::Serial ser;

void setupSerial(const std::string& port, uint32_t baud_rate) {
    try {
        ser.setPort(port);
        ser.setBaudrate(baud_rate);
        serial::Timeout to = serial::Timeout::simpleTimeout(1000);
        ser.setTimeout(to);
        ser.open();
        if (ser.isOpen()) {
            ROS_INFO_STREAM("Serial Port initialized");
        } else {
            ROS_ERROR_STREAM("Serial Port not open");
        }
    } catch (serial::IOException& e) {
        ROS_ERROR_STREAM("Unable to open port " << port);
        ros::shutdown();
    }
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "usbros_node");
    ros::NodeHandle nh;
    ros::Publisher dist_pub = nh.advertise<std_msgs::Float32>("distance", 1000);
    ros::Rate loop_rate(2);

    std::string port = "/dev/ttyUSB0"; // 修改为实际的串口
    uint32_t baud_rate = 9600;         // 修改为实际的波特率

    setupSerial(port, baud_rate);

    while (ros::ok()) {
        if (ser.available()) {
            std::string result = ser.read(ser.available());
            try {
                float distance = std::stof(result);
                float f_dx = 0.1168 * distance + 0.0699; // 计算 f_dx 的值
                std_msgs::Float32 dist_msg;
                dist_msg.data = f_dx; // 发布计算后的 f_dx 值
                dist_pub.publish(dist_msg);
                ROS_INFO("Distance: %f cm, f_dx: %f", distance, f_dx);
            } catch (std::invalid_argument& e) {
                ROS_WARN("Invalid data received: %s", result.c_str());
            }
        }
        ros::spinOnce();
        loop_rate.sleep();
    }
}
